//
//  ev3.h
//  ev3-c-api
//  (C) 2014 Codesign
//  distributed under LGPL
//

// This file defines only commands used in our small project. Nevertheless it shows how to communicate with the brick.
// The following packages are needed on Linux to compile the code: clang, libblocksruntime-dev, libdispatch-dev

#ifndef _ev3_h
#define _ev3_h

#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <dispatch/dispatch.h>
#include <pthread.h>
#include <Block.h>

#define _LITTLE_ENDIAN   0x41424344UL
#define _ENDIAN_ORDER    ('ABCD')

#if _ENDIAN_ORDER != _LITTLE_ENDIAN
#error Only little endian is supported
#endif

typedef enum {
    out_a = 0x01,
    out_b = 0x02,
    out_c = 0x04,
    out_d = 0x08,
} ev3_output_bitfield;

typedef enum {
    in_1 = 0,
    in_2 = 1,
    in_3 = 2,
    in_4 = 3,
} ev3_input_port;

typedef enum {
    motor_a = 0,
    motor_b = 1,
    motor_c = 2,
    motor_d = 3,
} ev3_motor_port;

typedef enum {
    input_device          = 0x99,
    input_read            = 0x9a,
    input_read_si         = 0x9d,
    output_stop           = 0xa3,
    output_power          = 0xa4,
    output_speed          = 0xa5,
    output_start          = 0xa6,
    output_read           = 0xa8,
    output_step_speed     = 0xae,
    output_clear_count    = 0xb2,
    output_get_count      = 0xb3,
} ev3_bytecode;

void* ev3_thread_entry(void* arg) {
    void(^block)() = (void(^)()) arg;
    block();
    Block_release(block);
    return NULL;
}

void ev3_detach_thread(void(^block)()) {
    pthread_t thread;
    pthread_create(&thread, NULL, &ev3_thread_entry, Block_copy(block));
    pthread_detach(thread);
}

struct ev3_connection {
    int fd;
    dispatch_semaphore_t sem;
};

struct ev3_connection* ev3_connection_open(const char* filename) {
    struct ev3_connection* conn = malloc(sizeof(struct ev3_connection));
    conn->fd = open(filename, O_RDWR);
    if (conn->fd != -1) conn->sem = dispatch_semaphore_create(1);
    return conn;
}

void ev3_connection_close(struct ev3_connection* conn) {
    if (conn->fd != -1) {
        dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
        close(conn->fd);
        dispatch_semaphore_signal(conn->sem);
        dispatch_release(conn->sem);
    }
    free(conn);
}

void ev3_send(struct ev3_connection* conn, int gvars, int lvars, int seq, int reply, char* data, int len) {
    int msglen = len + 5;
    char b1 = msglen, b2 = msglen >> 8;
    write(conn->fd, &b1, 1);
    write(conn->fd, &b2, 1);
    b1 = seq;
    b2 = seq >> 8;
    write(conn->fd, &b1, 1);
    write(conn->fd, &b2, 1);
    b1 = reply ? 0 : 0x80;
    write(conn->fd, &b1, 1);
    b1 = gvars & 0xff;
    b2 = ((lvars << 2) | (gvars >> 8));
    write(conn->fd, &b1, 1);
    write(conn->fd, &b2, 1);
    write(conn->fd, data, len);
}

void ev3_receive(struct ev3_connection* conn, void(^callback)(char*,int)) {
    char b1, b2;
    read(conn->fd, &b1, 1);
    read(conn->fd, &b2, 1);
    int len = (((int) b2) << 8) | b1;
    char data[len];
    read(conn->fd, data, len);
    callback(data, len);
}

void ev3_read_si(struct ev3_connection* conn, int seq, int chain, ev3_input_port port, int mode, void(^callback)(float)) {
    char data[7] = { input_read_si, chain, port, 0x80 | 0x01, 0, mode, 0x40 | 0x20 };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 4, 0, seq, 1, data, 7);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback(*((float*) (data + 3)));
    });
}

void ev3_read_si_f(struct ev3_connection* conn, int seq, int chain, ev3_input_port port, int mode, void(*callback)(float)) {
    ev3_read_si(conn, seq, chain, port, mode, ^(float val) { callback(val); });
}

void ev3_stop(struct ev3_connection* conn, int seq, int chain, ev3_output_bitfield motor, int brake, void(^callback)()) {
    char data[4] = { output_stop, chain, motor, brake ? 1 : 0 };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 0, 0, seq, 1, data, 4);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback();
    });
}

void ev3_clear_count(struct ev3_connection* conn, int seq, int chain, ev3_output_bitfield motor, void(^callback)()) {
    char data[3] = { output_clear_count, chain, motor };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 0, 0, seq, 1, data, 3);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback();
    });
}

void ev3_get_count(struct ev3_connection* conn, int seq, int chain, ev3_motor_port port, void(^callback)(int)) {
    char data[4] = { output_get_count, chain, port, 0x40 | 0x20 };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 4, 0, seq, 1, data, 4);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback(*(int*)(data + 3));
    });
}

void ev3_get_speed(struct ev3_connection* conn, int seq, int chain, ev3_motor_port port, void(^callback)(int)) {
    char data[5] = { output_read, chain, port, 0x40 | 0x20, 0x40 | 0x20 | 4 };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 8, 0, seq, 1, data, 5);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback((signed char) data[3]);
    });
}

void ev3_step(struct ev3_connection* conn, int seq, int chain, ev3_output_bitfield motor, int speed, int degrees, void(^callback)()) {
    int ramp_steps = 0.15 * degrees;
    int constant_steps = 0.7 * degrees;
    if (ramp_steps > 200) {
        int diff = ramp_steps - 200;
        ramp_steps = 200;
        constant_steps += 2 * diff;
    }
    char data[14] = { output_step_speed, chain, motor, speed > 0 ? speed : (0x20 - -speed) | 0x20,
        0x80 | 0x02, ramp_steps, ramp_steps >> 8, 0x80 | 0x02, constant_steps, constant_steps >> 8, 0x80 | 0x02, ramp_steps, ramp_steps >> 8, 0
    };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 0, 0, seq, 1, data, 14);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback();
    });
}

void ev3_double_step(struct ev3_connection* conn, int seq, int chain, ev3_output_bitfield motor1, int speed1, int degrees1, ev3_output_bitfield motor2, int speed2, int degrees2, void(^callback)()) {
    int ramp_steps1 = 0.15 * degrees1;
    int constant_steps1 = 0.7 * degrees1;
    int ramp_steps2 = 0.15 * degrees2;
    int constant_steps2 = 0.7 * degrees2;
    char data[28] = {
        output_step_speed, chain, motor1, speed1 > 0 ? speed1 : (0x20 - -speed1) | 0x20,
        0x80 | 0x02, ramp_steps1, ramp_steps1 >> 8, 0x80 | 0x02, constant_steps1, constant_steps1 >> 8, 0x80 | 0x02, ramp_steps1, ramp_steps1 >> 8, 0,
        output_step_speed, chain, motor2, speed2 > 0 ? speed2 : (0x20 - -speed2) | 0x20,
        0x80 | 0x02, ramp_steps2, ramp_steps2 >> 8, 0x80 | 0x02, constant_steps2, constant_steps2 >> 8, 0x80 | 0x02, ramp_steps2, ramp_steps2 >> 8, 0
    };
    dispatch_semaphore_wait(conn->sem, DISPATCH_TIME_FOREVER);
    ev3_send(conn, 0, 0, seq, 1, data, 28);
    ev3_receive(conn, ^(char* data, int len) {
        dispatch_semaphore_signal(conn->sem);
        callback();
    });
}

ev3_motor_port ev3_get_motor_port(ev3_output_bitfield bf) {
    switch (bf) {
        case out_a: return motor_a;
        case out_b: return motor_b;
        case out_c: return motor_c;
        case out_d: return motor_d;
    }
}

void ev3_step_and_notify(struct ev3_connection* conn, int seq, int chain, ev3_output_bitfield motor, int speed, int degrees, void(^callback)(int)) {
    ev3_step(conn, seq, chain, motor, speed, degrees, ^{
        ev3_detach_thread(^{
            __block int speed;
            while (1) {
                ev3_get_speed(conn, 0, 0, ev3_get_motor_port(motor), ^(int _speed) { speed = _speed; });
                if (speed == 0) break;
                usleep(100000);
            }
            ev3_get_count(conn, 0, 0, ev3_get_motor_port(motor), callback);
        });
    });
}

struct ev3_vehicle {
    float gauge;
    float wheel_diameter;
    struct ev3_connection* conn;
    ev3_output_bitfield left_motor;
    ev3_output_bitfield right_motor;
};

typedef struct ev3_vehicle* ev3_vehicle_t;

struct ev3_vehicle* ev3_vehicle_create(float gauge, int wheel_diameter, struct ev3_connection* conn, ev3_output_bitfield left_motor, ev3_output_bitfield right_motor) {
    struct ev3_vehicle* vehicle = malloc(sizeof(struct ev3_vehicle));
    vehicle->gauge = gauge;
    vehicle->wheel_diameter = wheel_diameter;
    vehicle->conn = conn;
    vehicle->left_motor = left_motor;
    vehicle->right_motor = right_motor;
    return vehicle;
}

void ev3_vehicle_close(struct ev3_vehicle* vehicle) {
    free(vehicle);
}

void ev3_vehicle_stop(struct ev3_vehicle* vehicle, void(^callback)()) {
    ev3_stop(vehicle->conn, 0, 0, vehicle->left_motor | vehicle->right_motor, 0, callback);
}

void ev3_vehicle_move(struct ev3_vehicle* vehicle, int speed, int distance, int(^stop_block)(), void(^callback)(int,int)) {
    int degrees = (distance / (vehicle->wheel_diameter * M_PI)) * 360;
    __block int running = 1;
    ev3_clear_count(vehicle->conn, 0, 0, vehicle->left_motor | vehicle->right_motor, ^{
        ev3_step(vehicle->conn, 0, 0, vehicle->left_motor | vehicle->right_motor, -speed, degrees, ^{
            ev3_detach_thread(^{
                __block int speed1, speed2;
                while (1) {
                    ev3_get_speed(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->left_motor), ^(int speed) { speed1 = speed; });
                    ev3_get_speed(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->right_motor), ^(int speed) { speed2 = speed; });
                    if (speed1 == 0 && speed2 == 0) break;
                    usleep(100000);
                }
                running = 0;
                ev3_get_count(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->left_motor), ^(int count1) {
                    ev3_get_count(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->right_motor), ^(int count2) {
                        //printf("## %d %d %d\n", degrees, count1, count2);
                        callback(-count1 / 360.0 * (vehicle->wheel_diameter * M_PI), -count2 / 360.0 * (vehicle->wheel_diameter * M_PI));
                    });
                });
            });
        });
    });
    if (stop_block != NULL) {
        ev3_detach_thread(^{
            while (running) {
                if (stop_block()) {
                    ev3_stop(vehicle->conn, 0, 0, vehicle->left_motor | vehicle->right_motor, 0, ^{
                        printf("emergency stop\n");
                    });
                    running  = false;
                }
                usleep(100000);
            }
        });
    }
}

void ev3_vehicle_spin(struct ev3_vehicle* vehicle, int speed, int angle, void(^callback)(int,int)) {
    int distance = (angle / 90.0) * (vehicle->gauge * M_PI / 4);
    int degrees = (distance / (vehicle->wheel_diameter * M_PI)) * 360;
    ev3_clear_count(vehicle->conn, 0, 0, vehicle->left_motor | vehicle->right_motor, ^{
        ev3_double_step(vehicle->conn, 0, 0, vehicle->left_motor, -speed, degrees, vehicle->right_motor, speed, degrees, ^{
            ev3_detach_thread(^{
                __block int speed1, speed2;
                while (1) {
                    ev3_get_speed(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->left_motor), ^(int speed) { speed1 = speed; });
                    ev3_get_speed(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->right_motor), ^(int speed) { speed2 = speed; });
                    if (speed1 == 0 && speed2 == 0) break;
                    usleep(100000);
                }
                ev3_get_count(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->left_motor), ^(int count1) {
                    ev3_get_count(vehicle->conn, 0, 0, ev3_get_motor_port(vehicle->right_motor), ^(int count2) {
                        int travelled1 = -count1 / 360.0 * (vehicle->wheel_diameter * M_PI), travelled2 = -count2 / 360.0 * (vehicle->wheel_diameter * M_PI);
                        callback(travelled1 / (vehicle->gauge * M_PI / 4) * 90, -travelled2 / (vehicle->gauge * M_PI / 4) * 90);
                    });
                });
            });
        });
    });
}

static dispatch_semaphore_t ev3_loop_sem;

void ev3_loop(void(^cleanup_block)()) {
    assert(ev3_loop_sem == NULL);
    ev3_loop_sem = dispatch_semaphore_create(0);
    dispatch_semaphore_wait(ev3_loop_sem, DISPATCH_TIME_FOREVER);
    cleanup_block();
    dispatch_release(ev3_loop_sem);
    ev3_loop_sem = NULL;
}

void ev3_finish() {
    assert(ev3_loop_sem != NULL);
    dispatch_semaphore_signal(ev3_loop_sem);
}

#endif
